package org.beltrw.MazeSolver;

import lejos.nxt.Button;
import lejos.nxt.ButtonListener;
import lejos.nxt.LightSensor;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.Sound;
import lejos.nxt.UltrasonicSensor;
import lejos.nxt.addon.CompassSensor;
import org.beltrw.Pilot.PinkRobotPilot;



/**
 * Class to solve a maze in the form of an array.
 *
 * @author Ryan Belt and Zach Joyner
 */
public class ArrayMazeSolver
{
    private static int[][] maze = {
	{1,1,1,1,1,1,1,1},
	{0,0,1,0,1,0,0,1},
	{1,0,1,0,0,0,1,1},
	{1,0,0,0,1,0,1,1},
	{1,0,1,0,1,0,0,0},
	{1,0,1,0,1,1,1,1},
	{1,0,1,0,0,0,0,1},
	{1,1,1,1,1,1,1,1}};
    private static final int START_ROW = 1;
    private static final int START_COL = 0;
    private static final char START_DIR = 'E';
    private static final int GOAL_ROW = 4;
    private static final int GOAL_COL = 7;
    private static int curr_row;
    private static int curr_col;
    private static char direction;
    private static final int WHEEL_DIAMETER = 58;
    private static final int TRACK_WIDTH = 160;
    private static final float TRAVEL_DISTANCE = 304.8f;
    private static PinkRobotPilot pink;
    private static CompassSensor cs = new CompassSensor(SensorPort.S3);
    private static LightSensor ls = new LightSensor(SensorPort.S1);
    private static UltrasonicSensor us = new UltrasonicSensor(SensorPort.S2);


    public static void main(String[] args)
    {
        Button.LEFT.addButtonListener(new KillSwitch());
        Button.RIGHT.addButtonListener(new KillSwitch());
        pink = new PinkRobotPilot(Motor.A, Motor.C, cs, TRACK_WIDTH, WHEEL_DIAMETER, true, ls, us);
        System.out.println("Solving...");
        solveMaze();
        System.out.println("Ready to run the maze");
        Button.waitForPress();
        runMaze();
        System.out.println("Finished Running");
        Button.waitForPress();
    }

    /**
     * Method to run the maze. Maze must be solved first
     */
    public static void runMaze()
    {
        curr_row = START_ROW;
        curr_col = START_COL;
        direction = START_DIR;
        int[] walls = new int[3];

        System.out.println("Running...");

        while(curr_row != GOAL_ROW || curr_col != GOAL_COL)
        {
            walls = getWalls2(curr_row, curr_col);
            if(walls[2] == 2)
            {
                // go right
                takeDir('r');
            }
            else if(walls[1] == 2)
            {
                // go straight
                takeDir('s');
            }
            else if(walls[0] == 2)
            {
                // go left
                takeDir('l');
            }
            else
            {
                // no paths, need to back track
                System.out.println("Stuck");
                Button.waitForPress();
                System.exit(0);
            }
        }
    }

    /**
     * Helper method to travel a specified direction
     * 
     * @param rOrL - 'r' for right, 'l' for left, 's' for straight
     */
    private static void takeDir(char rOrL)
    {
        switch(direction)
        {
            case 'N':
                switch(rOrL)
                {
                    case 'r':
                        direction = 'E';
                        curr_col += 1;
                        turnRightAndGoForward();
                        break;
                    case 'l':
                        direction = 'W';
                        curr_col -= 1;
                        turnLeftAndGoForward();
                        break;
                    case 's':
                        curr_row -= 1;
                        goForward();
                        break;
                }
                break;
                // END N
            case 'S':
                switch(rOrL)
                {
                    case 'r':
                        direction = 'W';
                        curr_col -= 1;
                        turnRightAndGoForward();
                        break;
                    case 'l':
                        direction = 'E';
                        curr_col += 1;
                        turnLeftAndGoForward();
                        break;
                    case 's':
                        curr_row += 1;
                        goForward();
                        break;
                }
                break;
                // END S
            case 'E':
                switch(rOrL)
                {
                    case 'r':
                        direction = 'S';
                        curr_row += 1;
                        turnRightAndGoForward();
                        break;
                    case 'l':
                        direction = 'N';
                        curr_row -= 1;
                        turnLeftAndGoForward();
                        break;
                    case 's':
                        curr_col += 1;
                        goForward();
                        break;
                }
                break;
                // END E
            case 'W':
                switch(rOrL)
                {
                    case 'r':
                        direction = 'N';
                        curr_row -= 1;
                        turnRightAndGoForward();
                        break;
                    case 'l':
                        direction = 'S';
                        curr_row += 1;
                        turnLeftAndGoForward();
                        break;
                    case 's':
                        curr_col -= 1;
                        goForward();
                        break;
                }
                break;
                // END W
        }
    }

    /**
     * Method to solve to maze
     */
    public static void solveMaze()
    {
        curr_row = START_ROW;
        curr_col = START_COL;
        direction = START_DIR;
        maze[curr_row][curr_col] = 2; // mark position
        int[] walls;

	//        System.out.println("To step through the solutions, press 'k' and then press Enter");
        //System.out.print("Press 'k' and Enter to continue:");
        //scan.next();
        //System.out.println();

        while(curr_row != GOAL_ROW || curr_col != GOAL_COL)
        {
            walls = getWalls(curr_row, curr_col);
            if(walls[2] == 0)
            {
                // go right
                goDir('r');
                //printMaze();
                //System.out.println();
                //System.out.print("Press 'k' and Enter to continue:");
                //scan.next();
                //System.out.println();
            }
            else if(walls[1] == 0)
            {
                // go straight
                goDir('s');
                //printMaze();
                //System.out.println();
                //System.out.print("Press 'k' and Enter to continue:");
                //scan.next();
                //System.out.println();
            }
            else if(walls[0] == 0)
            {
                // go left
                goDir('l');
                //printMaze();
                //System.out.println();
                //System.out.print("Press 'k' and Enter to continue:");
                //scan.next();
                //System.out.println();
            }
            else
            {
                // no paths, need to back track
                backtrack();
            }
        }
        System.out.println("Solution Found!");
        //System.out.println("Follow the 2's for the solution:\n");
        //printMaze();
    }

    /**
     * Helper method to get walls
     * 
     * @param row - current row
     * @param col - current column
     * @return - array of walls such that:
     *           arr[0] = left of dir
     *           arr[1] = straight
     *           arr[2] = right of dir
     */
    private static int[] getWalls(int row, int col)
    {
        int[] walls = new int[3];
        switch(direction)
        {
            case 'N':
                walls[0] = col > 0 ? maze[row][col-1] : -1;
                walls[1] = row > 0 ? maze[row-1][col] : -1;
                walls[2] = col < (maze[0].length-1) ? maze[row][col+1] : -1;
                break;
            case 'S':
                walls[0] = col < (maze[0].length-1) ? maze[row][col+1] : -1;
                walls[1] = row < (maze.length-1) ? maze[row+1][col] : -1;
                walls[2] = col > 0 ? maze[row][col-1] : -1;
                break;
            case 'E':
                walls[0] = row > 0 ? maze[row-1][col] : -1;
                walls[1] = col < (maze[0].length-1) ? maze[row][col+1] : -1;
                walls[2] = row < (maze.length-1) ? maze[row+1][col] : -1;
                break;
            case 'W':
                walls[0] = row < (maze.length-1) ? maze[row+1][col] : -1;
                walls[1] = col > 0 ? maze[row][col-1] : -1;
                walls[2] = row > 0 ? maze[row-1][col] : -1;
                break;
        }
        return walls;
    }

    /**
     * Helper method to get walls
     *
     * @param row - current row
     * @param col - current column
     * @return - array of walls such that:
     *           arr[0] = left of dir
     *           arr[1] = straight
     *           arr[2] = right of dir
     */
    private static int[] getWalls2(int row, int col)
    {
        int[] walls = new int[3];
        switch(direction)
        {
            case 'N':
                walls[0] = col > 0 ? maze[row][col-1] : -1;
                walls[1] = row > 0 ? maze[row-1][col] : -1;
                walls[2] = col < (maze[0].length-1) ? maze[row][col+1] : -1;
                break;
            case 'S':
                walls[0] = col < (maze[0].length-1) ? maze[row][col+1] : -1;
                walls[1] = row < (maze.length-1) ? maze[row+1][col] : -1;
                walls[2] = col > 0 ? maze[row][col-1] : -1;
                break;
            case 'E':
                walls[0] = row > 0 ? maze[row-1][col] : -1;
                walls[1] = col < (maze[0].length-1) ? maze[row][col+1] : -1;
                walls[2] = row < (maze.length-1) ? maze[row+1][col] : -1;
                break;
            case 'W':
                walls[0] = row < (maze.length-1) ? maze[row+1][col] : -1;
                walls[1] = col > 0 ? maze[row][col-1] : -1;
                walls[2] = row > 0 ? maze[row-1][col] : -1;
                break;
        }
        return walls;
    }

    /**
     * Prints the maze
     */
    private static void printMaze()
    {
        for(int i = 0; i < maze.length; i++)
        {
            for(int j = 0; j <maze[i].length; j++)
                System.out.print(maze[i][j]+" ");
            System.out.println();
        }
    }

    /**
     * "Moves" is the direction specified in parameter
     *
     * @param rOrL - char such that 'r' = right, 'l' = left, and 's' = straight
     */
    private static void goDir(char rOrL)
    {
        switch(direction)
        {
            case 'N':
                switch(rOrL)
                {
                    case 'r':
                        direction = 'E';
                        curr_col += 1;
                        break;
                    case 'l':
                        direction = 'W';
                        curr_col -= 1;
                        break;
                    case 's':
                        curr_row -= 1;
                        break;
                }
                break;
                // END N
            case 'S':
                switch(rOrL)
                {
                    case 'r':
                        direction = 'W';
                        curr_col -= 1;
                        break;
                    case 'l':
                        direction = 'E';
                        curr_col += 1;
                        break;
                    case 's':
                        curr_row += 1;
                        break;
                }
                break;
                // END S
            case 'E':
                switch(rOrL)
                {
                    case 'r':
                        direction = 'S';
                        curr_row += 1;
                        break;
                    case 'l':
                        direction = 'N';
                        curr_row -= 1;
                        break;
                    case 's':
                        curr_col += 1;
                        break;
                }
                break;
                // END E
            case 'W':
                switch(rOrL)
                {
                    case 'r':
                        direction = 'N';
                        curr_row -= 1;
                        break;
                    case 'l':
                        direction = 'S';
                        curr_row += 1;
                        break;
                    case 's':
                        curr_col -= 1;
                        break;
                }
                break;
                // END W
        }
        maze[curr_row][curr_col] = 2; // mark position
    }

    /**
     * Method to backtrack if robot gets stuck
     */
    private static void backtrack()
    {
        maze[curr_row][curr_col] = 3; // mark position as bad
        switch(direction)
        {
            case 'N':
                direction = 'S';
                break;
            case 'S':
                direction = 'N';
                break;
            case 'E':
                direction = 'W';
                break;
            case 'W':
                direction = 'E';
                break;
        }
        int[] walls = getWalls(curr_row, curr_col);
        // loop until you come to a junction
        while(walls[2] != 0 && walls[1] != 0 && walls[0] != 0)
        {
            maze[curr_row][curr_col] = 3; // mark position as bad
            if(walls[2] == 2)
            {
                // go right
                goDir('r');
            }
            else if(walls[1] == 2)
            {
                // go straight
                goDir('s');
            }
            else if(walls[0] == 2)
            {
                // go left
                goDir('l');

            }
            //printMaze();
            //System.out.println();
            //System.out.print("Press 'k' and Enter to continue:");
            //scan.next();
            //System.out.println();
            walls = getWalls(curr_row, curr_col);
        }
    }




    static class KillSwitch implements ButtonListener
    {
        public void buttonPressed(Button b)
        {
            Sound.beepSequence();
            System.exit(0);
        }

        public void buttonReleased(Button b) {}
    }

    private static void turnLeft() {
        //Configureation.getPilot().rotate(90f);
        pink.turnLeft90();
    }

    private static void turnRight() {
        //Configureation.getPilot().rotate(-90f);
        pink.turnRight90();
    }

    public static void goForward() {
        //Configureation.getPilot().travel((float) (15.5 * Constants.INtoCM * Constants.CMtoMM));
        pink.forwardACell(TRAVEL_DISTANCE);
    }

    public static void turnLeftAndGoForward() {
        turnLeft();
        goForward();
    }

    public static void turnRightAndGoForward() {
        turnRight();
        goForward();
    }
}
